<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.11"/>
<title>Cheetah Software: DynamicsSimulator.cpp Source File</title>
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<link href="../../navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../resize.js"></script>
<script type="text/javascript" src="../../navtreedata.js"></script>
<script type="text/javascript" src="../../navtree.js"></script>
<script type="text/javascript">
  $(document).ready(initResizable);
  $(window).load(resizeHeight);
</script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { init_search(); });
</script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Cheetah Software
   &#160;<span id="projectnumber">1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.11 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="../../index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="../../namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="../../annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="../../files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="../../search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="../../search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="../../files.html"><span>File&#160;List</span></a></li>
      <li><a href="../../globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(document).ready(function(){initNavTree('de/d4c/_dynamics_simulator_8cpp_source.html','../../');});
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">DynamicsSimulator.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<a href="../../de/d4c/_dynamics_simulator_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;</div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d0/d52/_dynamics_simulator_8h.html">Dynamics/DynamicsSimulator.h</a>&quot;</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../da/daa/_contact_impulse_8h.html">Collision/ContactImpulse.h</a>&quot;</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../da/d04/_contact_spring_damper_8h.html">Collision/ContactSpringDamper.h</a>&quot;</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="../../d3/d68/_utilities__print_8h.html">Utilities/Utilities_print.h</a>&quot;</span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;</div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00017"></a><span class="lineno"><a class="line" href="../../d7/d9c/class_dynamics_simulator.html#a232389d37139581b88f2730ae99d1de9">   17</a></span>&#160;<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a232389d37139581b88f2730ae99d1de9">DynamicsSimulator&lt;T&gt;::DynamicsSimulator</a>(<a class="code" href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel&lt;T&gt;</a> &amp;model,</div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;                                        <span class="keywordtype">bool</span> useSpringDamper)</div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;    : _model(model), _useSpringDamper(useSpringDamper) {</div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1aeb05200221fc3f4edbbd2c1fc3bfb4">_useSpringDamper</a>) {</div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#abd202861bda97c9e0d05cee7e6bdf61d">_contact_constr</a> = <span class="keyword">new</span> <a class="code" href="../../d0/df2/class_contact_spring_damper.html">ContactSpringDamper&lt;T&gt;</a>(&amp;<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>);</div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;  } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#abd202861bda97c9e0d05cee7e6bdf61d">_contact_constr</a> = <span class="keyword">new</span> <a class="code" href="../../d2/dc6/class_contact_impulse.html">ContactImpulse&lt;T&gt;</a>(&amp;<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>);</div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;  }</div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyPosition = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation = <a class="code" href="../../d9/d8e/cpp_types_8h.html#a8686862774f7c3186c7920447f4de282">Quat&lt;T&gt;::Zero</a>();</div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.q = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>._nDof - 6);</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.qd = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>._nDof - 6);</div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.qdd = <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;::Zero</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>._nDof - 6);</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#aca5ea569f4c05ecfb4f8bfcf4ef08378">_lastBodyVelocity</a>.setZero();</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;}</div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00041"></a><span class="lineno"><a class="line" href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">   41</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">DynamicsSimulator&lt;T&gt;::step</a>(T dt, <span class="keyword">const</span> <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec&lt;T&gt;</a> &amp;tau, T kp, T kd ) {</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;  <span class="comment">// fwd-kin on gc points</span></div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;  <span class="comment">// compute ground contact forces</span></div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;  <span class="comment">// aba</span></div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;  <span class="comment">// integrate</span></div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a3c057386e8d437e8c5dbcae02acf935c">forwardKinematics</a>();           <span class="comment">// compute forward kinematics</span></div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ac2a79fc7a13298b6fa4b9fa87844106e">updateCollisions</a>(dt, kp, kd);  <span class="comment">// process collisions</span></div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;  <span class="comment">// Process Homing</span></div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;  <span class="keywordflow">if</span>( <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.active_flag) {</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> R10_des = <a class="code" href="../../da/d9b/namespaceori.html#add867f41c8046a8bba05611e25c8dfcd">rpyToRotMat</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.rpy);              <span class="comment">// R10_des</span></div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> R10_act = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.getOrientation(5).transpose();  <span class="comment">// R10</span></div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3&lt;T&gt;</a> eR01 = R10_des.transpose()*R10_act;              <span class="comment">// eR * R01 = R01_des</span></div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;    </div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a738072d0d6a7a632be68f6261501826b">Vec4&lt;T&gt;</a> equat = <a class="code" href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">rotationMatrixToQuaternion</a>(eR01.transpose());</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> angle_axis = <a class="code" href="../../da/d9b/namespaceori.html#a90e9fca31a184b48790b43aa6f44fcbd">quatToso3</a>(equat); <span class="comment">// in world frame</span></div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> p = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.getPosition(5);</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> <a class="code" href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a> = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.kp_lin*(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.position - p)-<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.kd_lin*<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.getLinearVelocity(5);</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;    <span class="comment">// Note: External forces are spatial forces in the {0} frame. </span></div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>._externalForces.at(5) += <a class="code" href="../../de/d1a/namespacespatial.html#a3b32f3f809891285d9f1a8cf3c7cd16e">forceToSpatialForce</a>(f,p);</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>._externalForces.at(5).head(3) += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.kp_ang*angle_axis - <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">_homing</a>.kd_ang*<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.getAngularVelocity(5);</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;  }</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;</div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab7c1b6d1eb8d4893458b6910b1f9d331">runABA</a>(tau);                   <span class="comment">// dynamics algorithm</span></div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#aa713195f636bdfd32743ad5cd5385fcd">integrate</a>(dt);                 <span class="comment">// step forward</span></div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.setState(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>);</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.resetExternalForces();  <span class="comment">// clear external forces</span></div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.resetCalculationFlags();</div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;}</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00077"></a><span class="lineno"><a class="line" href="../../d7/d9c/class_dynamics_simulator.html#ac2a79fc7a13298b6fa4b9fa87844106e">   77</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ac2a79fc7a13298b6fa4b9fa87844106e">DynamicsSimulator&lt;T&gt;::updateCollisions</a>(T dt, T kp, T kd) {</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">_model</a>.forwardKinematics();</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;  <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#abd202861bda97c9e0d05cee7e6bdf61d">_contact_constr</a>-&gt;UpdateExternalForces(kp, kd, dt);</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;}</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> T&gt;</div><div class="line"><a name="l00087"></a><span class="lineno"><a class="line" href="../../d7/d9c/class_dynamics_simulator.html#aa713195f636bdfd32743ad5cd5385fcd">   87</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#aa713195f636bdfd32743ad5cd5385fcd">DynamicsSimulator&lt;T&gt;::integrate</a>(T dt) {</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;  <span class="keywordflow">if</span> (<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a1aeb05200221fc3f4edbbd2c1fc3bfb4">_useSpringDamper</a>) {</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> omegaBody = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity.template block&lt;3, 1&gt;(0, 0);</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6&lt;T&gt;</a> <a class="code" href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">X</a> = <a class="code" href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">createSXform</a>(<a class="code" href="../../da/d9b/namespaceori.html#af88fa91d686c646d41f9b747a6bc90f0">quaternionToRotationMatrix</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation),</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;                             <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyPosition);</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;T&gt;</a> R = <a class="code" href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">rotationFromSXform</a>(X);</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> omega0 = R.transpose() * omegaBody;</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;    <span class="comment">// actual integration</span></div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.qd += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.qdd * dt;</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.q += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.qd * dt;</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyVelocity * dt;</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyPosition += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyPosition * dt;</div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation = <a class="code" href="../../da/d9b/namespaceori.html#ac12ecf143879de178929ecaad1714a6a">integrateQuat</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation, omega0, dt);</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;  } <span class="keywordflow">else</span> {</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;    <span class="comment">// actual integration</span></div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;    <span class="comment">// Velocity Update by integrating acceleration</span></div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.qd += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.qdd * dt;</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyVelocity * dt;</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;</div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;    <span class="comment">// Contact Constraint Velocity Updated</span></div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#abd202861bda97c9e0d05cee7e6bdf61d">_contact_constr</a>-&gt;UpdateQdot(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>);</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;    <span class="comment">// Prepare body velocity integration</span></div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat&lt;T&gt;</a> R_body = <a class="code" href="../../da/d9b/namespaceori.html#af88fa91d686c646d41f9b747a6bc90f0">quaternionToRotationMatrix</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation);</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyPosition =</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;        R_body.transpose() * <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity.template block&lt;3, 1&gt;(3, 0);</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;    <a class="code" href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3&lt;T&gt;</a> omegaBody = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity.template block&lt;3, 1&gt;(0, 0);</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;    <span class="comment">// Position Update</span></div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.q += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.qd * dt;</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyPosition += <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyPosition * dt;</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation =</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;        <a class="code" href="../../da/d9b/namespaceori.html#a24ac869b814e7e46a63c4ee97e3279ea">integrateQuatImplicit</a>(<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyOrientation, omegaBody, dt);</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">_dstate</a>.dBodyVelocity = (<a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity - <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#aca5ea569f4c05ecfb4f8bfcf4ef08378">_lastBodyVelocity</a>) / dt;</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;    <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#aca5ea569f4c05ecfb4f8bfcf4ef08378">_lastBodyVelocity</a> = <a class="code" href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">_state</a>.bodyVelocity;</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;  }</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;}</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;<span class="keyword">template</span> <span class="keyword">class </span><a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;double&gt;</a>;</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;<span class="keyword">template</span> <span class="keyword">class </span><a class="code" href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt;float&gt;</a>;</div><div class="ttc" id="namespacespatial_html_a3b32f3f809891285d9f1a8cf3c7cd16e"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#a3b32f3f809891285d9f1a8cf3c7cd16e">spatial::forceToSpatialForce</a></div><div class="ttdeci">auto forceToSpatialForce(const Eigen::MatrixBase&lt; T &gt; &amp;f, const Eigen::MatrixBase&lt; T2 &gt; &amp;p)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00348">spatial.h:348</a></div></div>
<div class="ttc" id="namespaceori_html_a24ac869b814e7e46a63c4ee97e3279ea"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a24ac869b814e7e46a63c4ee97e3279ea">ori::integrateQuatImplicit</a></div><div class="ttdeci">Quat&lt; typename T::Scalar &gt; integrateQuatImplicit(const Eigen::MatrixBase&lt; T &gt; &amp;quat, const Eigen::MatrixBase&lt; T2 &gt; &amp;omega, T3 dt)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00315">orientation_tools.h:315</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a232389d37139581b88f2730ae99d1de9"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a232389d37139581b88f2730ae99d1de9">DynamicsSimulator::DynamicsSimulator</a></div><div class="ttdeci">EIGEN_MAKE_ALIGNED_OPERATOR_NEW DynamicsSimulator(FloatingBaseModel&lt; T &gt; &amp;model, bool useSpringDamper=false)</div><div class="ttdef"><b>Definition:</b> <a href="../../de/d4c/_dynamics_simulator_8cpp_source.html#l00017">DynamicsSimulator.cpp:17</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a3c057386e8d437e8c5dbcae02acf935c"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a3c057386e8d437e8c5dbcae02acf935c">DynamicsSimulator::forwardKinematics</a></div><div class="ttdeci">void forwardKinematics()</div><div class="ttdoc">Do forward kinematics for feet. </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00053">DynamicsSimulator.h:53</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html">DynamicsSimulator&lt; double &gt;</a></div></div>
<div class="ttc" id="namespaceori_html_add867f41c8046a8bba05611e25c8dfcd"><div class="ttname"><a href="../../da/d9b/namespaceori.html#add867f41c8046a8bba05611e25c8dfcd">ori::rpyToRotMat</a></div><div class="ttdeci">Mat3&lt; typename T::Scalar &gt; rpyToRotMat(const Eigen::MatrixBase&lt; T &gt; &amp;v)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00083">orientation_tools.h:83</a></div></div>
<div class="ttc" id="_contact_spring_damper_8h_html"><div class="ttname"><a href="../../da/d04/_contact_spring_damper_8h.html">ContactSpringDamper.h</a></div><div class="ttdoc">Spring Damper based Contact Computation logic. </div></div>
<div class="ttc" id="cpp_types_8h_html_ae8ab6bd4079a5317297be5eeb331b1ca"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae8ab6bd4079a5317297be5eeb331b1ca">Mat6</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 6 &gt; Mat6</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00070">cppTypes.h:70</a></div></div>
<div class="ttc" id="cpp_types_8h_html_aa8cb424e643d476ea485238dad7c3e18"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#aa8cb424e643d476ea485238dad7c3e18">Mat3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; Mat3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00054">cppTypes.h:54</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a8686862774f7c3186c7920447f4de282"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a8686862774f7c3186c7920447f4de282">Quat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 4, 1 &gt; Quat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00058">cppTypes.h:58</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a6f07728c866b15ff0676a9b2ebfd2ba6"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a6f07728c866b15ff0676a9b2ebfd2ba6">Vec3</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 1 &gt; Vec3</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00026">cppTypes.h:26</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_ab18a1180b0e999ac385177f361922ad9"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#ab18a1180b0e999ac385177f361922ad9">DynamicsSimulator::_dstate</a></div><div class="ttdeci">FBModelStateDerivative&lt; T &gt; _dstate</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00123">DynamicsSimulator.h:123</a></div></div>
<div class="ttc" id="_utilities__print_8h_html"><div class="ttname"><a href="../../d3/d68/_utilities__print_8h.html">Utilities_print.h</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_aa713195f636bdfd32743ad5cd5385fcd"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#aa713195f636bdfd32743ad5cd5385fcd">DynamicsSimulator::integrate</a></div><div class="ttdeci">void integrate(T dt)</div><div class="ttdoc">Integrate to find new _state. </div><div class="ttdef"><b>Definition:</b> <a href="../../de/d4c/_dynamics_simulator_8cpp_source.html#l00087">DynamicsSimulator.cpp:87</a></div></div>
<div class="ttc" id="class_contact_impulse_html"><div class="ttname"><a href="../../d2/dc6/class_contact_impulse.html">ContactImpulse</a></div><div class="ttdef"><b>Definition:</b> <a href="../../da/daa/_contact_impulse_8h_source.html#l00015">ContactImpulse.h:15</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a1aeb05200221fc3f4edbbd2c1fc3bfb4"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a1aeb05200221fc3f4edbbd2c1fc3bfb4">DynamicsSimulator::_useSpringDamper</a></div><div class="ttdeci">bool _useSpringDamper</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00128">DynamicsSimulator.h:128</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a3d70f8eb9cf1855ca989f887a5543acc"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a3d70f8eb9cf1855ca989f887a5543acc">SVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 6, 1 &gt; SVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00062">cppTypes.h:62</a></div></div>
<div class="ttc" id="namespaceori_html_ac12ecf143879de178929ecaad1714a6a"><div class="ttname"><a href="../../da/d9b/namespaceori.html#ac12ecf143879de178929ecaad1714a6a">ori::integrateQuat</a></div><div class="ttdeci">Quat&lt; typename T::Scalar &gt; integrateQuat(const Eigen::MatrixBase&lt; T &gt; &amp;quat, const Eigen::MatrixBase&lt; T2 &gt; &amp;omega, T3 dt)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00283">orientation_tools.h:283</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_ac2a79fc7a13298b6fa4b9fa87844106e"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#ac2a79fc7a13298b6fa4b9fa87844106e">DynamicsSimulator::updateCollisions</a></div><div class="ttdeci">void updateCollisions(T dt, T kp, T kd)</div><div class="ttdef"><b>Definition:</b> <a href="../../de/d4c/_dynamics_simulator_8cpp_source.html#l00077">DynamicsSimulator.cpp:77</a></div></div>
<div class="ttc" id="namespacespatial_html_ae4a2145f3028087511b4f17c0152ec83"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#ae4a2145f3028087511b4f17c0152ec83">spatial::createSXform</a></div><div class="ttdeci">auto createSXform(const Eigen::MatrixBase&lt; T &gt; &amp;R, const Eigen::MatrixBase&lt; T2 &gt; &amp;r)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00140">spatial.h:140</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a9da22c6a15e3786276a6cf395bfd725d"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a9da22c6a15e3786276a6cf395bfd725d">DynamicsSimulator::_homing</a></div><div class="ttdeci">RobotHomingInfo&lt; T &gt; _homing</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00130">DynamicsSimulator.h:130</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_abd202861bda97c9e0d05cee7e6bdf61d"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#abd202861bda97c9e0d05cee7e6bdf61d">DynamicsSimulator::_contact_constr</a></div><div class="ttdeci">ContactConstraint&lt; T &gt; * _contact_constr</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00126">DynamicsSimulator.h:126</a></div></div>
<div class="ttc" id="namespaceori_html_a73c3872af6870d856cedd662856fcaea"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a73c3872af6870d856cedd662856fcaea">ori::rotationMatrixToQuaternion</a></div><div class="ttdeci">Quat&lt; typename T::Scalar &gt; rotationMatrixToQuaternion(const Eigen::MatrixBase&lt; T &gt; &amp;r1)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00119">orientation_tools.h:119</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a738072d0d6a7a632be68f6261501826b"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a738072d0d6a7a632be68f6261501826b">Vec4</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 4, 1 &gt; Vec4</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00030">cppTypes.h:30</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a84b25acaf7edf23f4f437a96a0d8ba05"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a84b25acaf7edf23f4f437a96a0d8ba05">DynamicsSimulator::step</a></div><div class="ttdeci">void step(T dt, const DVec&lt; T &gt; &amp;tau, T kp, T kd)</div><div class="ttdoc">Initialize simulator with given model. </div><div class="ttdef"><b>Definition:</b> <a href="../../de/d4c/_dynamics_simulator_8cpp_source.html#l00041">DynamicsSimulator.cpp:41</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_ab7c1b6d1eb8d4893458b6910b1f9d331"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#ab7c1b6d1eb8d4893458b6910b1f9d331">DynamicsSimulator::runABA</a></div><div class="ttdeci">void runABA(const DVec&lt; T &gt; &amp;tau)</div><div class="ttdoc">Simulate forward one step. </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00050">DynamicsSimulator.h:50</a></div></div>
<div class="ttc" id="namespaceori_html_a90e9fca31a184b48790b43aa6f44fcbd"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a90e9fca31a184b48790b43aa6f44fcbd">ori::quatToso3</a></div><div class="ttdeci">Vec3&lt; typename T::Scalar &gt; quatToso3(const Eigen::MatrixBase&lt; T &gt; &amp;q)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00213">orientation_tools.h:213</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_aca5ea569f4c05ecfb4f8bfcf4ef08378"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#aca5ea569f4c05ecfb4f8bfcf4ef08378">DynamicsSimulator::_lastBodyVelocity</a></div><div class="ttdeci">SVec&lt; T &gt; _lastBodyVelocity</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00127">DynamicsSimulator.h:127</a></div></div>
<div class="ttc" id="class_floating_base_model_html"><div class="ttname"><a href="../../d6/db1/class_floating_base_model.html">FloatingBaseModel</a></div><div class="ttdef"><b>Definition:</b> <a href="../../d5/d9a/_floating_base_model_8h_source.html#l00069">FloatingBaseModel.h:69</a></div></div>
<div class="ttc" id="cpp_types_8h_html_ae2e5a0a178a2c528d8aa939f85db7a3e"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#ae2e5a0a178a2c528d8aa939f85db7a3e">DVec</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, Eigen::Dynamic, 1 &gt; DVec</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00102">cppTypes.h:102</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a51adda43ba61205140f8b4086de0b335"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a51adda43ba61205140f8b4086de0b335">DynamicsSimulator::_model</a></div><div class="ttdeci">FloatingBaseModel&lt; T &gt; &amp; _model</div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00124">DynamicsSimulator.h:124</a></div></div>
<div class="ttc" id="class_dynamics_simulator_html_a846cc5a17a011db74bcfd77c0e8e25cb"><div class="ttname"><a href="../../d7/d9c/class_dynamics_simulator.html#a846cc5a17a011db74bcfd77c0e8e25cb">DynamicsSimulator::_state</a></div><div class="ttdeci">FBModelState&lt; T &gt; _state</div><div class="ttdoc">Update ground collision list. </div><div class="ttdef"><b>Definition:</b> <a href="../../d0/d52/_dynamics_simulator_8h_source.html#l00122">DynamicsSimulator.h:122</a></div></div>
<div class="ttc" id="_dynamics_simulator_8h_html"><div class="ttname"><a href="../../d0/d52/_dynamics_simulator_8h.html">DynamicsSimulator.h</a></div><div class="ttdoc">Rigid Body Dynamics Simulator with Collisions. </div></div>
<div class="ttc" id="namespaceori_html_af88fa91d686c646d41f9b747a6bc90f0"><div class="ttname"><a href="../../da/d9b/namespaceori.html#af88fa91d686c646d41f9b747a6bc90f0">ori::quaternionToRotationMatrix</a></div><div class="ttdeci">Mat3&lt; typename T::Scalar &gt; quaternionToRotationMatrix(const Eigen::MatrixBase&lt; T &gt; &amp;q)</div><div class="ttdef"><b>Definition:</b> <a href="../../d4/dfb/orientation__tools_8h_source.html#l00160">orientation_tools.h:160</a></div></div>
<div class="ttc" id="_contact_impulse_8h_html"><div class="ttname"><a href="../../da/daa/_contact_impulse_8h.html">ContactImpulse.h</a></div></div>
<div class="ttc" id="namespaceori_html_a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383"><div class="ttname"><a href="../../da/d9b/namespaceori.html#a02c20a1b1d35988c657aa1e219d609d5a02129bb861061d1a052c592e2dc6b383">ori::CoordinateAxis::X</a></div></div>
<div class="ttc" id="class_contact_spring_damper_html"><div class="ttname"><a href="../../d0/df2/class_contact_spring_damper.html">ContactSpringDamper</a></div><div class="ttdef"><b>Definition:</b> <a href="../../da/d04/_contact_spring_damper_8h_source.html#l00012">ContactSpringDamper.h:12</a></div></div>
<div class="ttc" id="cpp_types_8h_html_a0b83d93a3c74d282558436e26c056896"><div class="ttname"><a href="../../d9/d8e/cpp_types_8h.html#a0b83d93a3c74d282558436e26c056896">RotMat</a></div><div class="ttdeci">typename Eigen::Matrix&lt; T, 3, 3 &gt; RotMat</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/d8e/cpp_types_8h_source.html#l00018">cppTypes.h:18</a></div></div>
<div class="ttc" id="namespacespatial_html_abdc3cd991206414d80d9f89be27e3633"><div class="ttname"><a href="../../de/d1a/namespacespatial.html#abdc3cd991206414d80d9f89be27e3633">spatial::rotationFromSXform</a></div><div class="ttdeci">auto rotationFromSXform(const Eigen::MatrixBase&lt; T &gt; &amp;X)</div><div class="ttdef"><b>Definition:</b> <a href="../../d9/dc8/spatial_8h_source.html#l00157">spatial.h:157</a></div></div>
<div class="ttc" id="test__casadi_8cpp_html_ae20d7fba59865a21504b65cdb44cbfe5"><div class="ttname"><a href="../../dc/dd7/test__casadi_8cpp.html#ae20d7fba59865a21504b65cdb44cbfe5">f</a></div><div class="ttdeci">MX f(const MX &amp;x, const MX &amp;u)</div><div class="ttdef"><b>Definition:</b> <a href="../../dc/dd7/test__casadi_8cpp_source.html#l00202">test_casadi.cpp:202</a></div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="../../dir_43e0a1f539e00dcfa1a6bc4d4fee4fc2.html">home</a></li><li class="navelem"><a class="el" href="../../dir_61bfb0b3cdda81b1b0355ba34eb3b1eb.html">dhkim</a></li><li class="navelem"><a class="el" href="../../dir_adee5b101e079090017bb4e91c628ead.html">Public_Repository</a></li><li class="navelem"><a class="el" href="../../dir_7fc07a7b97001e7b1d18e11da761e2de.html">Cheetah-Software</a></li><li class="navelem"><a class="el" href="../../dir_860379c26524a894b3cf97b5b1ba5bdb.html">common</a></li><li class="navelem"><a class="el" href="../../dir_054bbb67b14bc4d9ad4b8f9fd2c17d22.html">src</a></li><li class="navelem"><a class="el" href="../../dir_15eb938ed49f19e0fef569b69def6abf.html">Dynamics</a></li><li class="navelem"><a class="el" href="../../de/d4c/_dynamics_simulator_8cpp.html">DynamicsSimulator.cpp</a></li>
    <li class="footer">Generated by
    <a href="http://www.doxygen.org/index.html">
    <img class="footer" src="../../doxygen.png" alt="doxygen"/></a> 1.8.11 </li>
  </ul>
</div>
</body>
</html>
